#include <fmt/core.h>
#include <string>
#include <atomic>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <queue>

#include <chrono>
#include <nlohmann/json.hpp>
#include <opencv2/opencv.hpp>
#include "io/camera.hpp"
#include "serial/serial.h"
#include "tasks/auto_aim/aimer.hpp"
#include "tasks/auto_aim/solver.hpp"
#include "tasks/auto_aim/tracker.hpp"
#include "tasks/auto_aim/yolo.hpp"
#include "tools/exiter.hpp"
#include "tools/img_tools.hpp"
#include "tools/logger.hpp"
#include "tools/math_tools.hpp"
#include "tools/plotter.hpp"
#include "tools/math_tools.hpp"

// 线程安全队列模板
template<typename T>
class ThreadSafeQueue {
private:
    std::queue<T> queue_;
    mutable std::mutex mutex_;
    std::condition_variable cond_;
    size_t max_size_;
    
public:
    ThreadSafeQueue(size_t max_size = 10) : max_size_(max_size) {}
    
    void push(T value) {
        std::lock_guard<std::mutex> lock(mutex_);
        if (queue_.size() >= max_size_) {
            queue_.pop(); // 如果队列满，丢弃最旧的数据
        }
        queue_.push(std::move(value));
        cond_.notify_one();
    }
    
    bool pop(T& value) {
        std::unique_lock<std::mutex> lock(mutex_);
        if (cond_.wait_for(lock, std::chrono::milliseconds(1), 
                          [this] { return !queue_.empty(); })) {
            value = std::move(queue_.front());
            queue_.pop();
            return true;
        }
        return false;
    }
    
    bool empty() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return queue_.empty();
    }
    
    size_t size() const {
        std::lock_guard<std::mutex> lock(mutex_);
        return queue_.size();
    }
};

// 图像数据和时间戳结构
struct ImageData {
    cv::Mat image;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 检测结果结构
struct DetectionResult {
    std::list<auto_aim::Armor> armors;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 跟踪结果结构
struct TrackingResult {
    std::list<auto_aim::Target> targets;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 瞄准结果结构
struct AimingResult {
    io::Command command;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 串口数据包结构
struct SerialPacket {
    double delta_yaw;
    double delta_pitch;
    double horizon_distance;
    bool control;
    bool shoot;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

// 整车规划结果结构
struct VehiclePlanningResult {
    std::list<auto_aim::Target> targets;
    io::Command command;
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
    double w, x, y, z; // 四元数数据
};

// 显示数据结构
struct DisplayData {
    cv::Mat image;
    std::list<auto_aim::Target> targets;
    io::Command command;
    double w, x, y, z; // 四元数数据
    std::chrono::steady_clock::time_point timestamp;
    int frame_id;
};

int main(int argc, char * argv[])
{
    // 手动解析命令行参数
    std::string config_path = "configs/sentry.yaml";
    
    for (int i = 1; i < argc; i++) {
        std::string arg = argv[i];
        
        if (arg == "-h" || arg == "--help" || arg == "-?" || arg == "--usage") {
            std::cout << "用法: " << argv[0] << " [配置文件路径]" << std::endl;
            std::cout << "参数说明:" << std::endl;
            std::cout << "  配置文件路径: yaml配置文件路径 (默认: configs/sentry.yaml)" << std::endl;
            return 0;
        }
        else if (arg[0] != '-') {
            config_path = arg;
        }
    }

    tools::logger()->info("Starting parallel pipeline USB camera mode with config: {}", config_path);
    tools::Exiter exiter;

    // 初始化摄像头
    io::Camera camera(config_path);

    // 初始化串口
    serial::Serial serial_port;
    try {
        serial_port.setPort("/dev/ttyACM0");
        serial_port.setBaudrate(115200);
        serial_port.setTimeout(serial::Timeout::max(), 10, 0, 10, 0); // 减少超时
        serial_port.open();
        tools::logger()->info("Serial port /dev/ttyACM0 opened successfully");
    } catch (const std::exception & e) {
        tools::logger()->error("Failed to open serial port: {}", e.what());
        return -1;
    }

    // 初始化YOLO、Solver、Tracker、Aimer（重新启用模型检测）
    auto_aim::YOLO yolo(config_path);
    auto_aim::Solver solver(config_path);
    auto_aim::Tracker tracker(config_path, solver);
    auto_aim::Aimer aimer(config_path);

    // 创建线程安全队列（减少队列深度，降低延迟）
    ThreadSafeQueue<ImageData> image_queue(2);        // 图像队列，减少到2帧
    ThreadSafeQueue<SerialPacket> serial_queue(5);      // 串口数据队列，减少到5个包
    ThreadSafeQueue<VehiclePlanningResult> planning_queue(3); // 整车规划队列
    ThreadSafeQueue<DisplayData> display_queue(2);     // 显示数据队列
    
    // 原子变量控制线程退出
    std::atomic<bool> quit{false};
    
    // 四元数数据（使用原子变量，线程安全）
    std::atomic<double> w{1.0}, x{0.0}, y{0.0}, z{0.0};
    
    // 缓存变量（供串口线程使用）
    std::atomic<double> last_delta_yaw{0.0};
    std::atomic<double> last_delta_pitch{0.0};
    std::atomic<bool> last_control{false};
    std::atomic<bool> last_shoot{false};
    std::atomic<double> last_horizon_distance{0.0};
    
    // 性能统计
    std::atomic<int> camera_frame_count{0};
    std::atomic<int> detection_frame_count{0};
    std::atomic<int> serial_frame_count{0};
    std::atomic<int> display_frame_count{0};
    
    auto start_time = std::chrono::steady_clock::now();
    int global_frame_id = 0;

    // 线程1：摄像头读取线程（包含完整的四元数读取和整车规划）
    auto camera_thread = std::thread([&]() {
        tools::logger()->info("Camera thread started (with full quaternion reading and vehicle planning)");
        
        // 默认四元数（如果串口读取失败时使用）
        double current_w = 1.0, current_x = 0.0, current_y = 0.0, current_z = 0.0;
        
        while (!quit) {
            auto thread_start = std::chrono::steady_clock::now();
            
            try {
                cv::Mat img;
                std::chrono::steady_clock::time_point timestamp;
                
                // 从串口读取四元数数据（完整18字节解析）
                if (serial_port.isOpen()) {
                    try {
                        // 先读取缓冲区中的所有可用数据
                        std::vector<uint8_t> all_data;
                        int available_bytes = serial_port.available();
                        
                        // 添加调试信息：显示缓冲区状态
                        tools::logger()->info("Serial buffer has {} bytes available", available_bytes);
                        
                        serial_port.read(all_data, available_bytes);
                        
                        // 添加调试信息：显示实际读取到的数据
                        if (!all_data.empty()) {
                            tools::logger()->info("Read {} bytes from serial port", all_data.size());
                            
                            // 显示前几个字节的内容（用于调试）
                            if (all_data.size() >= 5) {
                                tools::logger()->info("First 5 bytes: 0x{:02X} 0x{:02X} 0x{:02X} 0x{:02X} 0x{:02X}", 
                                                     all_data[0], all_data[1], all_data[2], all_data[3], all_data[4]);
                            }
                        }
                        
                        if (all_data.size() >= 18) {
                            // 查找有效的数据帧
                            bool frame_found = false;
                            for (size_t i = 0; i <= all_data.size() - 18; ++i) {
                                // 检查帧头和帧尾
                                if (all_data[i] == 0xAA && all_data[i + 17] == 0xFF) {
                                    // 找到有效帧，提取18字节数据
                                    std::vector<uint8_t> buffer(all_data.begin() + i, all_data.begin() + i + 18);
                                    
                                    // 解析4个float值 (w, x, y, z)
                                    union {
                                        uint8_t bytes[4];
                                        float value;
                                    } converter;
                                    
                                    // 解析w
                                    converter.bytes[0] = buffer[1];
                                    converter.bytes[1] = buffer[2];
                                    converter.bytes[2] = buffer[3];
                                    converter.bytes[3] = buffer[4];
                                    current_w = converter.value;
                                    
                                    // 解析x
                                    converter.bytes[0] = buffer[5];
                                    converter.bytes[1] = buffer[6];
                                    converter.bytes[2] = buffer[7];
                                    converter.bytes[3] = buffer[8];
                                    current_x = converter.value;
                                    
                                    // 解析y
                                    converter.bytes[0] = buffer[9];
                                    converter.bytes[1] = buffer[10];
                                    converter.bytes[2] = buffer[11];
                                    converter.bytes[3] = buffer[12];
                                    current_y = converter.value;
                                    
                                    // 解析z
                                    converter.bytes[0] = buffer[13];
                                    converter.bytes[1] = buffer[14];
                                    converter.bytes[2] = buffer[15];
                                    converter.bytes[3] = buffer[16];
                                    current_z = converter.value;
                                    
                                    // 更新原子变量
                                    w.store(current_w);
                                    x.store(current_x);
                                    y.store(current_y);
                                    z.store(current_z);
                                    
                                    // 打印接收到的四元数数据
                                    tools::logger()->info("Received quaternion: w={:.6f}, x={:.6f}, y={:.6f}, z={:.6f}", 
                                                         current_w, current_x, current_y, current_z);
                                    
                                    frame_found = true;
                                    break; // 找到一个有效帧就退出
                                }
                            }
                            
                            // 如果没有找到有效帧，打印调试信息
                            if (!frame_found && all_data.size() >= 18) {
                                tools::logger()->info("No valid quaternion frame found in {} bytes of data", all_data.size());
                                tools::logger()->info("Expected frame format: 0xAA [w_bytes] [x_bytes] [y_bytes] [z_bytes] 0xFF");
                            }
                        } else if (all_data.size() > 0) {
                            tools::logger()->info("Received {} bytes (need 18 bytes for quaternion frame)", all_data.size());
                        }
                    } catch (const std::exception & e) {
                        tools::logger()->warn("Error reading from serial port: {}", e.what());
                    }
                }
                
                // 读取摄像头数据
                camera.read(img, timestamp);
                
                if (!img.empty()) {
                    int frame_id = ++global_frame_id;
                    
                    // 将数据推送到队列（仅用于显示）
                    if (image_queue.size() < 2) {
                        image_queue.push({img.clone(), timestamp, frame_id});
                    }
                    camera_frame_count++;
                    
                    // 进行模型检测（包含整车规划）
                    auto detection_start = std::chrono::steady_clock::now();
                    
                    // 1. YOLO检测
                    auto armors = yolo.detect(img, frame_id);
                    
                    // 2. 目标跟踪
                    auto targets = tracker.track(armors, timestamp);
                    
                    // 3. 设置四元数到solver
                    solver.set_R_gimbal2world({current_w, current_x, current_y, current_z});
                    
                    // 4. 瞄准计算
                    auto command = aimer.aim(targets, timestamp, 27, false);
                    
                    // 如果没有检测到装甲板，强制设置shoot=0
                    if (targets.empty()) {
                        command.shoot = false;
                        command.control = false;
                    }
                    
                    // 5. 更新缓存的控制指令（供串口线程使用）
                    if (targets.empty() || !command.control) {
                        last_delta_yaw.store(0.0);
                        last_delta_pitch.store(0.0);
                        last_control.store(false);
                        last_shoot.store(false);
                        last_horizon_distance.store(0.0);  // 重置水平距离
                        
                        // 添加调试信息：显示为什么发送默认数据
                        if (targets.empty()) {
                            tools::logger()->warn("No targets detected, sending default data");
                        } else {
                            tools::logger()->warn("Control disabled, sending default data");
                        }
                    } else {
                        // 计算当前云台的yaw和pitch角度
                        Eigen::Quaterniond gimbal_q(current_w, current_x, current_y, current_z);
                        Eigen::Vector3d gimbal_ypr = tools::eulers(gimbal_q, 2, 1, 0);
                        double current_yaw = gimbal_ypr[0];
                        double current_pitch = gimbal_ypr[2];  // 使用roll值作为pitch
                        
                        // 计算角度差值
                        double delta_yaw_val = command.yaw - current_yaw;
                        double delta_pitch_val = command.pitch - current_pitch;
                        
                        // 限制差值范围
                        delta_yaw_val = tools::limit_rad(delta_yaw_val);
                        delta_pitch_val = tools::limit_rad(delta_pitch_val);
                        
                        last_delta_yaw.store(delta_yaw_val);
                        last_delta_pitch.store(delta_pitch_val);
                        last_control.store(command.control);
                        last_shoot.store(command.shoot);
                        last_horizon_distance.store(command.horizon_distance);  // 更新水平距离
                        
                        // 添加调试信息：显示检测到的目标信息
                        tools::logger()->info("Target detected: {} targets, control enabled, delta_yaw={:.4f}, delta_pitch={:.4f}, horizon_distance={:.2f}", 
                                              targets.size(), delta_yaw_val, delta_pitch_val, command.horizon_distance);
                    }
                    
                    // 6. 将整车规划结果推送到队列
                    VehiclePlanningResult planning_result;
                    planning_result.targets = targets;
                    planning_result.command = command;
                    planning_result.timestamp = timestamp;
                    planning_result.frame_id = frame_id;
                    planning_result.w = current_w;
                    planning_result.x = current_x;
                    planning_result.y = current_y;
                    planning_result.z = current_z;
                    
                    // 移除队列大小限制，始终推送规划数据
                    planning_queue.push(planning_result);
                    
                    // 7. 将显示数据推送到队列
                    DisplayData display_data;
                    display_data.image = img.clone();
                    display_data.targets = targets;
                    display_data.command = command;
                    display_data.w = current_w;
                    display_data.x = current_x;
                    display_data.y = current_y;
                    display_data.z = current_z;
                    display_data.timestamp = timestamp;
                    display_data.frame_id = frame_id;
                    
                    // 移除显示队列大小限制
                    display_queue.push(display_data);
                    
                    detection_frame_count++;
                    
                    auto detection_time = std::chrono::duration_cast<std::chrono::microseconds>(
                        std::chrono::steady_clock::now() - detection_start);
                    
                    if (frame_id % 100 == 0) {  // 减少日志频率
                        tools::logger()->info("Detection time: {:.1f}ms", detection_time.count() / 1000.0);
                    }
                }
                
            } catch (const std::exception & e) {
                tools::logger()->warn("Camera read failed: {}", e.what());
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
                continue;
            }
            
            // 控制摄像头读取频率（约100Hz）
            auto thread_end = std::chrono::steady_clock::now();
            auto thread_time = std::chrono::duration_cast<std::chrono::microseconds>(
                thread_end - thread_start);
            
            if (thread_time < std::chrono::microseconds(10000)) { // 10ms间隔
                std::this_thread::sleep_for(std::chrono::microseconds(10000) - thread_time);
            }
        }
        
        tools::logger()->info("Camera thread stopped");
    });



    // 线程2：串口发送线程（高频发送，使用实际检测结果）
    auto serial_thread = std::thread([&]() {
        tools::logger()->info("Serial thread started (using real detection results)");
        
        while (!quit) {
            auto thread_start = std::chrono::steady_clock::now();
            
            // 实时发送数据（高频发送）
            if (serial_port.isOpen()) {
                try {
                    // 获取最新的四元数数据
                    double current_w = w.load();
                    double current_x = x.load();
                    double current_y = y.load();
                    double current_z = z.load();
                    
                    // 计算当前云台的yaw和pitch角度
                    Eigen::Quaterniond gimbal_q(current_w, current_x, current_y, current_z);
                    Eigen::Vector3d gimbal_ypr = tools::eulers(gimbal_q, 2, 1, 0);
                    double current_yaw = gimbal_ypr[0];
                    double current_pitch = gimbal_ypr[2];  // 使用roll值作为pitch
                    
                    // 获取最新的目标检测结果（从原子变量）
                    double delta_yaw = last_delta_yaw.load();
                    double delta_pitch = last_delta_pitch.load();
                    bool control = last_control.load();
                    bool shoot = last_shoot.load();
                    double horizon_distance = last_horizon_distance.load();
                    
                    // 快速打包数据
                    uint8_t frame_data[13] = {
                        0xAA, 2, 1, 
                        static_cast<uint8_t>(control ? 1 : 0), 
                        static_cast<uint8_t>(shoot ? 1 : 0),
                        static_cast<uint8_t>((int16_t)(delta_yaw * 57.3 * 100) & 0xFF),
                        static_cast<uint8_t>(((int16_t)(delta_yaw * 57.3 * 100) >> 8) & 0xFF),
                        static_cast<uint8_t>((int16_t)(delta_pitch * 57.3 * 100) & 0xFF),
                        static_cast<uint8_t>(((int16_t)(delta_pitch * 57.3 * 100) >> 8) & 0xFF),
                        static_cast<uint8_t>((int16_t)(horizon_distance * 10000) & 0xFF),
                        static_cast<uint8_t>(((int16_t)(horizon_distance * 10000) >> 8) & 0xFF),
                        254, 0xBB   // 质量+帧尾
                    };
                    
                    // 发送数据前的调试信息 - 打印十六进制数据
                    std::string sent_hex_dump;
                    for (int i = 0; i < 13; i++) {
                        sent_hex_dump += fmt::format("{:02X} ", frame_data[i]);
                    }
                    tools::logger()->info("Sending serial data (hex): {}", sent_hex_dump);
                    
                    // 发送数据
                    auto serial_start = std::chrono::steady_clock::now();
                    serial_port.write(frame_data, 13);
                    auto serial_end = std::chrono::steady_clock::now();
                    
                    // 计算发送耗时（微秒）
                    double serial_time_us = std::chrono::duration_cast<std::chrono::microseconds>(
                        serial_end - serial_start).count();
                    
                    // 调试信息（每帧都输出调试信息）
                    tools::logger()->info(
                        "Serial sent: control={}, shoot={}, delta_yaw={:.4f}({:.2f}°), delta_pitch={:.4f}({:.2f}°), horizon={:.2f}, time={:.2f}μs",
                        control, shoot, 
                        delta_yaw, delta_yaw * 57.3,
                        delta_pitch, delta_pitch * 57.3,
                        horizon_distance, serial_time_us);
                    
                    serial_frame_count++;
                    
                } catch (const std::exception & e) {
                    tools::logger()->warn("Error sending data to serial port: {}", e.what());
                }
            }
            
            // 控制串口发送频率（500Hz，约2ms间隔）
            auto thread_end = std::chrono::steady_clock::now();
            auto thread_time = std::chrono::duration_cast<std::chrono::microseconds>(
                thread_end - thread_start);
            
            if (thread_time < std::chrono::microseconds(2000)) { // 2ms间隔
                std::this_thread::sleep_for(std::chrono::microseconds(2000) - thread_time);
            }
        }
        
        tools::logger()->info("Serial thread stopped");
    });

    // 主线程：显示和协调（包含重投影和详细数据显示）
    tools::logger()->info("Display thread started (with reprojection and detailed data display)");
    
    cv::Mat img;
    auto t0 = std::chrono::steady_clock::now();
    int frame_count = 0;
    
    // JSON数据记录
    nlohmann::json data;
    auto_aim::Target last_target;
    double last_t = -1;
    
    while (!exiter.exit()) {
        DisplayData display_data;
        if (display_queue.pop(display_data)) {
            img = display_data.image;
            
            if (img.empty()) {
                continue;
            }

            // 性能统计显示
            auto current_time = std::chrono::steady_clock::now();
            double elapsed_seconds = std::chrono::duration<double>(current_time - start_time).count();
            
            double camera_fps = camera_frame_count / elapsed_seconds;
            double detection_fps = detection_frame_count / elapsed_seconds;
            double serial_fps = serial_frame_count / elapsed_seconds;
            double display_fps = display_frame_count / elapsed_seconds;
            
            tools::draw_text(
                img,
                fmt::format("Camera FPS: {:.1f}, Detection FPS: {:.1f}", camera_fps, detection_fps),
                {10, 30}, {0, 255, 0});
            tools::draw_text(
                img,
                fmt::format("Serial FPS: {:.1f}, Display FPS: {:.1f}", serial_fps, display_fps),
                {10, 60}, {0, 255, 0});

            // 显示控制指令信息
            tools::draw_text(
                img,
                fmt::format("Command: {},{:.2f},{:.2f},shoot:{}", 
                          display_data.command.control, display_data.command.yaw * 57.3,
                          display_data.command.pitch * 57.3, display_data.command.shoot),
                {10, 90}, {154, 50, 205});

            // 显示四元数解析角度信息
            Eigen::Quaterniond gimbal_q(display_data.w, display_data.x, display_data.y, display_data.z);
            Eigen::Vector3d gimbal_ypr = tools::eulers(gimbal_q, 2, 1, 0);
            tools::draw_text(
                img,
                fmt::format("Gimbal Yaw: {:.2f}°", gimbal_ypr[0] * 57.3),
                {10, 120}, {255, 255, 255});
            tools::draw_text(
                img,
                fmt::format("Gimbal Pitch: {:.2f}°", gimbal_ypr[2] * 57.3),
                {10, 150}, {255, 255, 255});
            tools::draw_text(
                img,
                fmt::format("Gimbal Roll: {:.2f}°", gimbal_ypr[1] * 57.3),
                {10, 180}, {255, 255, 255});

            // 重投影功能：绘制装甲板四个顶点和瞄准点
            if (!display_data.targets.empty()) {
                auto target = display_data.targets.front();
                
                // 计算相对时间（秒）
                double t = std::chrono::duration<double>(display_data.timestamp - t0).count();
                
                if (last_t == -1) {
                    last_target = target;
                    last_t = t;
                }

                // 装甲板重投影绘制
                std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
                for (const Eigen::Vector4d & xyza : armor_xyza_list) {
                    auto image_points = solver.reproject_armor(xyza.head(3), xyza[3], 
                                                             target.armor_type, target.name);
                    tools::draw_points(img, image_points, {0, 255, 0}); // 绿色装甲板顶点
                }

                // 瞄准点重投影绘制
                auto aim_point = aimer.debug_aim_point;
                Eigen::Vector4d aim_xyza = aim_point.xyza;
                auto image_points = solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], 
                                                         target.armor_type, target.name);
                if (aim_point.valid) tools::draw_points(img, image_points, {0, 0, 255}); // 蓝色瞄准点

                // JSON数据记录
                data["frame_id"] = display_data.frame_id;
                data["timestamp"] = std::chrono::duration_cast<std::chrono::microseconds>(
                    display_data.timestamp.time_since_epoch()).count();
                data["armor_num"] = display_data.targets.size();
                
                // 记录云台角度信息
                data["gimbal_yaw"] = gimbal_ypr[0] * 57.3;
                data["gimbal_pitch"] = gimbal_ypr[2] * 57.3;
                data["gimbal_roll"] = gimbal_ypr[1] * 57.3;
                
                // 记录四元数数据
                data["quaternion_w"] = display_data.w;
                data["quaternion_x"] = display_data.x;
                data["quaternion_y"] = display_data.y;
                data["quaternion_z"] = display_data.z;
                
                // 记录控制指令
                data["control_enable"] = display_data.command.control;
                data["shoot_enable"] = display_data.command.shoot;
                data["target_yaw"] = display_data.command.yaw * 57.3;
                data["target_pitch"] = display_data.command.pitch * 57.3;
                data["horizon_distance"] = display_data.command.horizon_distance;
                
                // 记录装甲板信息（如果有目标）
                if (!display_data.targets.empty()) {
                    const auto& target = display_data.targets.front();
                    // 获取装甲板列表
                    auto armor_list = target.armor_xyza_list();
                    if (!armor_list.empty()) {
                        const auto& armor = armor_list[0];
                        // 装甲板的世界坐标
                        data["armor_x"] = armor[0];
                        data["armor_y"] = armor[1];
                        data["armor_z"] = armor[2];
                        data["armor_yaw"] = armor[3] * 57.3;
                        
                        // 记录EKF状态
                        auto ekf_x = target.ekf_x();
                        data["ekf_center_x"] = ekf_x[0];
                        data["ekf_center_y"] = ekf_x[2];
                        data["ekf_center_z"] = ekf_x[4];
                        data["ekf_angle"] = ekf_x[6] * 57.3;
                        data["ekf_angular_velocity"] = ekf_x[7] * 57.3;
                        data["ekf_radius"] = ekf_x[8];
                    }
                }

                // // 输出JSON数据（每10帧输出一次）
                // if (frame_count % 10 == 0) {
                //     tools::logger()->info("JSON Data: {}", data.dump());
                // }

                last_target = target;
                last_t = t;
            }

            cv::resize(img, img, {}, 0.5, 0.5);
            cv::imshow("Parallel Pipeline USB Camera (with Reprojection)", img);
            auto key = cv::waitKey(1);
            if (key == 'q') break;
            
            display_frame_count++;
            frame_count++;
        } else {
            // 没有图像数据时短暂休眠
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }

    // 清理线程
    quit = true;
    if (camera_thread.joinable()) camera_thread.join();
    if (serial_thread.joinable()) serial_thread.join();

    // 关闭串口
    if (serial_port.isOpen()) {
        serial_port.close();
    }

    tools::logger()->info("Simplified YOLO detection pipeline stopped");
    return 0;
}